{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 107,
   "id": "ccb98fb3",
   "metadata": {},
   "outputs": [],
   "source": [
    "from trlc_dk1.leader import DK1Leader, DK1LeaderConfig\n",
    "\n",
    "leader_config = DK1LeaderConfig(\n",
    "    port=\"/dev/tty.usbmodem58FA0824311\"\n",
    ")\n",
    "\n",
    "leader = DK1Leader(leader_config)\n",
    "leader.connect()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8a1a3414",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "{'joint_1': 2016, 'joint_2': 2048, 'joint_3': 2050, 'joint_4': 2047, 'joint_5': 2044, 'joint_6': 2108}\n",
      "joint_1: 32\n",
      "joint_2: 0\n",
      "joint_3: -2\n",
      "joint_4: 1\n",
      "joint_5: 4\n",
      "joint_6: -60\n"
     ]
    }
   ],
   "source": [
    "present_position = leader.bus.sync_read(normalize=False, data_name=\"Present_Position\")\n",
    "print(present_position)\n",
    "\n",
    "zero_position = {\"joint_1\": 2048, \"joint_2\": 2048, \"joint_3\": 2048, \"joint_4\": 2048, \"joint_5\": 2048, \"joint_6\": 2048}\n",
    "\n",
    "for joint, pos in present_position.items():\n",
    "    offset = zero_position[joint] - pos\n",
    "    print(f\"{joint}: {offset}\")\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 109,
   "id": "ba2e5ed6",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'joint_1': 0,\n",
       " 'joint_2': 23,\n",
       " 'joint_3': 22,\n",
       " 'joint_4': 22,\n",
       " 'joint_5': 0,\n",
       " 'joint_6': 0}"
      ]
     },
     "execution_count": 109,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "leader.bus.sync_read(\"Homing_Offset\", normalize=False)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 110,
   "id": "790f8c70",
   "metadata": {},
   "outputs": [],
   "source": [
    "leader.bus.write(\"Homing_Offset\", \"joint_4\", 22)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "dk1",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.18"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
